package com.DataProcess;

import android.util.Log;

import com.arithmetic.InitMap;
import com.arithmetic.InitNative;
import com.arithmetic.MotorsControl;
import com.pudu.mydemo.nativeClass.NativeClass;

import java.util.Arrays;

/**
 * Created by tjpu on 2016/10/12.
 */
public class ClientDataProcess {
    static String thisLogTag = "ClientDataProcess";

    ClientData clientData = new ClientData();
    public void putOrigin(byte[] dataSource) {
        byte [] data = new byte[dataSource.length];
        System.arraycopy(dataSource,0,data,0,data.length);
        clientData.decode(data);
        switch (clientData.type) {
            case ClientDataType.FLAG:
                flag();
                break;
            case ClientDataType.CONTROL:
                control();
                break;
            case ClientDataType.PID:
                pid();
                break;
            case ClientDataType.OPTIMIZE:
                optimize();      // 设置算法的优化参数
                break;
            case ClientDataType.FAIL:
                break;
            default:
        }
    }

    void flag() {
        if (clientData.isRefresh) {
            Robot.isInitPosSucceed = true;
            InitMap.initMap();
            InitNative.initNative();
            clientData.isRefresh = false;
        }
        Robot.isAuto = clientData.isAutomatic;
        Robot.isCloseLoop = clientData.isCloseLoop;
        Robot.isEnable = clientData.isEnable;

        //出现速度丢帧保护时，遥控器可以重新启动
        if (clientData.isEnable) {
//            speedDataProcess.notReceiveCnt = 0;
//            speedDataProcess.isNotReceive = false;
        }

        if (!clientData.isAutomatic) {
            Robot.targetSpeed[0] = 0.0;
            Robot.targetSpeed[1] = 0.0;
        }
    }


    void control() {
        Log.i(thisLogTag,"target debug" + "isAuto: " + Robot.isAuto);
        if (!Robot.isAuto) {
            Robot.targetSpeed[0] = clientData.targetSpeedL * Robot.MAX_CLIENT_SPEED;
            Robot.targetSpeed[1] = clientData.targetSpeedR * Robot.MAX_CLIENT_SPEED;
        }
        Log.i(thisLogTag,"target debug"+Arrays.toString(Robot.targetSpeed) + "    " + clientData.targetSpeedL);
    }

    static final double PIDScale = 10.0;
    void pid() {
        MotorsControl.left.setPID(clientData.PID[0]/PIDScale,clientData.PID[1]/PIDScale,clientData.PID[2]/PIDScale);
        MotorsControl.right.setPID(clientData.PID[0]/PIDScale,clientData.PID[1]/PIDScale,clientData.PID[2]/PIDScale);
        Log.i(thisLogTag,"PID:"+ Arrays.toString(clientData.PID));
    }

    void optimize() {
        if (clientData.optimizeK < 7) {
            if(clientData.optimizeK == 5)
                clientData.optimizeValue = clientData.optimizeValue /100.0;
            NativeClass.cSetK(clientData.optimizeK, clientData.optimizeValue);
        }
        if (clientData.optimizeK == 8) {
            NativeClass.cSetKp(clientData.optimizeValue);
        }
        if (clientData.optimizeK == 9) {
            NativeClass.cSetKd(clientData.optimizeValue);
        }
        Log.i(thisLogTag,"optimize K:"+clientData.optimizeK+" value:"+clientData.optimizeValue);
    }
}
